/**
 * @file trapezoid.c
 * @author cvrookie
 * @brief 
 * @version 0.1
 * @date 2023-12-20
 * 
 * @copyright Copyright (c) 2023
 * @note 步进电机梯形加速算法头文件
 *       本模块支持重入,同时支持多个电机加减速
 *       本模块支持的电机最小步距角或者步长为0.01，速度精度只支持2位小数
 */

#include <stdint.h>
#include <stdio.h>
#include <stdbool.h>
#include <string.h>
#include "math.h"
#include "stdlib.h"
#include "trapezoid.h"


static void TrapeCal_Reset(Trapecal_medium  *trapemedium);
static inline void TrapeCal_CalAccelStep(Trapecal_medium  *trapemedium);
static inline void TrapeCal_CalAccelLimitStep(Trapecal_medium  *trapemedium);
static inline void TrapeCal_CalDecelStep(Trapecal_medium  *trapemedium);
static inline uint32_t TrapeCal_CalFirstCounterVal(Trapecal_medium  *trapemedium);
static inline uint32_t TrapeCal_CalCounterValAndRest(Trapecal_medium  *trapemedium);



/**
 * @brief 梯形算法的计算初始化函数
 *
 * @note 通过初始设置得到中间的计算参数
 *       初始设置会放到步进电机的实例里面，中间参数也是
 *       在中断里计算的时候再遍历所有实例吧
 */
Trapecal_medium  TrapeCal_Init(Trapecal_param *trapeconfig)
{
    Trapecal_medium trape;
    trape.stepsize = trapeconfig->stepsize;   
    trape.ClkFrq = trapeconfig->ClkFrq;
    trape.accelSpeed = (uint32_t)(trapeconfig->accelSpeed*100);
    trape.decelSpeed = (uint32_t)(trapeconfig->decelSpeed*100);
    trape.constSpeed = (uint32_t)(trapeconfig->constSpeed*100);
    trape.total_step = trapeconfig->total_step;

    return trape;
}


/**
 * @brief 开始计算梯形加减速相关参数的函数，中间的借个过程也被函数模块化了
 * 
 * @param Trapecal_medium *trapemedium 中间计算会用到的一些参数，后续会加入到步进电机的实例里（希望指针不要出错）
 *
 * @retval 返回第一个Counter值
 */
uint32_t TrapeCal_Start(Trapecal_medium  *trapemedium)
{
    uint32_t CounterVal;
	
	TrapeCal_Reset(trapemedium);

	TrapeCal_CalAccelStep(trapemedium);

	TrapeCal_CalAccelLimitStep(trapemedium);

	TrapeCal_CalDecelStep(trapemedium);

    CounterVal = TrapeCal_CalFirstCounterVal(trapemedium);

    return CounterVal;
   
}

/**
 * @brief 复位函数
 */
static void TrapeCal_Reset(Trapecal_medium  *trapemedium)
{
    trapemedium->status = ACCEL;
	trapemedium->rest = 0;
    trapemedium->stepCount = 0;
	trapemedium->prevCounterVal = 0;

}

/**
 * @brief 获取加速阶段步数的函数
 * 
 * @retval 根据情况返回加速阶段的步数
 */
uint32_t TrapeCal_GetAccelSteps(Trapecal_medium  *trapemedium)
{
    if(trapemedium->accelStep > trapemedium->accelLimStep)
    {
        return trapemedium->accelLimStep;
    }
    else
    {
        return trapemedium->accelStep;
    }
}

/**
 * @brief 获取减速阶段步数的函数
 * 
 * @retval 返回减速阶段的步数
 */
uint32_t TrapeCal_GetdecelSteps(Trapecal_medium  *trapemedium)
{
   return (uint32_t)(-trapemedium->decelStep);
}

/**
 * @brief 获取匀速阶段的步数
 * 
 * @retval 返回减速阶段的步数
 */
uint32_t TrapeCal_GetConstSteps(Trapecal_medium  *trapemedium)
{
   return (uint32_t)((int32_t)trapemedium->total_step-(int32_t)trapemedium->accelStep+trapemedium->decelStep);
   //decelStep已经是负的的了所以直接加
}
 
/**
 * @brief 获取已经运行的步数
 *
 * @retval 返回已运行的步数
 */
uint32_t TrapeCal_GetStepCount(Trapecal_medium  *trapemedium)
{   
    if(trapemedium->status == DECEL)
    {
        return (uint32_t)((int32_t)trapemedium->total_step + trapemedium->stepCount+1);
    }
    else
    {
        return (uint32_t)trapemedium->stepCount;
    }
}

/**
 * @brief 获取运行状态
 * 
 * @retval 返回运行状态
 */
TRAPE_ACCEL_STAT_t TrapeCal_GetStatus(Trapecal_medium  *trapemedium)
{
    return trapemedium->status;
}

/**
 * @brief 计算加速到最大速度时候的步数
 * 
 */
static inline void TrapeCal_CalAccelStep(Trapecal_medium  *trapemedium)
{
    trapemedium->accelStep = (uint64_t)trapemedium->constSpeed * (uint64_t)trapemedium->constSpeed 
                        / (2 * trapemedium->accelSpeed * (uint32_t)(trapemedium->stepsize * 100));
    //进行变量类型的强制转换是因为平方后可能会出现大数据，32位不够储存
}

/**
 * @brief 计算加速极限的步数
 */
static inline void TrapeCal_CalAccelLimitStep(Trapecal_medium  *trapemedium)
{
    trapemedium->accelLimStep = (uint64_t)trapemedium->accelSpeed * (uint64_t)trapemedium->total_step 
									/ ( trapemedium->accelSpeed + trapemedium->decelSpeed);  
}

/**
 * @brief 计算减速阶段的步数
 * 
 * @note  此函数必须在TrapeAccel_CalAccelStep和TrapeAccel_CalAccelStep之后调用
 */
static inline void TrapeCal_CalDecelStep(Trapecal_medium  *trapemedium)
{

    if(trapemedium->accelLimStep > trapemedium->accelStep)
    {
        trapemedium->decelStep = -(int32_t)((uint64_t)trapemedium->accelStep * (uint64_t)trapemedium->accelSpeed / trapemedium->decelSpeed);     
    }
    else
    {
        trapemedium->decelStep =  (int32_t)trapemedium->accelLimStep - (int32_t)trapemedium->total_step;
    }

}

/**
 * @brief 计算电机加速运行时的初始计数器值
 *
 * @retval 返回计数器值
 */
static inline uint32_t TrapeCal_CalFirstCounterVal(Trapecal_medium  *trapemedium)
{
    uint32_t T1_FQEQ;
    uint64_t  A_SQ;
    uint32_t counterVal;

    T1_FQEQ = (uint32_t)(0.00676 * trapemedium->ClkFrq);

    A_SQ = (uint64_t)(20000000000LLU * (double)trapemedium->stepsize );
	
    counterVal = (uint32_t)(T1_FQEQ * sqrt(A_SQ /(uint64_t)trapemedium->accelSpeed)*0.01);
    trapemedium->prevCounterVal = counterVal;

    return counterVal;
}


/**
 * @brief 计算电机下一个周期的计数器值
 *
 * @retval 计数器值
 */
static inline uint32_t TrapeCal_CalCounterValAndRest(Trapecal_medium  *trapemedium)
{
    uint32_t temp;

    temp = (uint32_t)((int32_t)trapemedium->prevCounterVal -
			(2 * (int32_t)trapemedium->prevCounterVal + trapemedium->rest) / (4*trapemedium->stepCount + 1));

    trapemedium->rest = (2 * (int32_t)trapemedium->prevCounterVal + trapemedium->rest) % (4*trapemedium->stepCount + 1);
    
    trapemedium->prevCounterVal = temp;

    return temp; 
}

/**
 * @brief 计算下一步
 * 
 * @retval 返回电机状态。stop时停止计算
 */
TRAPE_ACCEL_STAT_t  TrapeCal_CalNextCounterVal(Trapecal_medium  *trapemedium)
{
    trapemedium->stepCount++;
    switch(trapemedium->status)
    {

        case ACCEL:
			if((uint32_t)trapemedium->stepCount > trapemedium->accelLimStep)
			{
				trapemedium->status = DECEL;
				trapemedium->stepCount = trapemedium->decelStep;
				break;
			}
			else if((uint32_t)trapemedium->stepCount > trapemedium->accelStep)
			{
				trapemedium->status = CONSTANT;
				break;
			}
            trapemedium->presentCounterVal = TrapeCal_CalCounterValAndRest(trapemedium);
            break;
        case CONSTANT:
            trapemedium->presentCounterVal = trapemedium->prevCounterVal;
            if((uint32_t)trapemedium->stepCount > (uint32_t)((int32_t)trapemedium->total_step + trapemedium->decelStep))
            {
                trapemedium->status = DECEL;
                trapemedium->stepCount = trapemedium->decelStep;
                break;
            }

            break;
        case DECEL:
            if(trapemedium->stepCount == 0)
            {
                trapemedium->status = STOP;
                break;
            }
            trapemedium->presentCounterVal = TrapeCal_CalCounterValAndRest(trapemedium);

            break;
        case STOP:
            trapemedium->presentCounterVal = 0;
            TrapeCal_Reset(trapemedium); 
            break;
        
    }
    return trapemedium->status;
}

